#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

(ros::roseus "eusmodel_to_trimesh_ordered_pcloud")

(ros::roseus-add-msgs "sensor_msgs")
(ros::roseus-add-msgs "object_manipulation_msgs")
(ros::roseus-add-srvs "object_manipulation_msgs")

(load "package://jskpointcloud/src/euslisp/init.l")

(unless (boundp '*pickview*) (pickview :no-menu t))

(defun eusmodel-to-trimesh-ordered-pcloud (mdl &key (return-eus3dp t) (input-trimesh t))
  (let* ((fs (if input-trimesh mdl (body-to-triangles mdl)))
         (trimesh-lst (send fs :faces))
         (pt-lst (list))
         (indices-lst (range (* 3 (length trimesh-lst))))
         3dp ret)
    (dolist (f trimesh-lst)
      (setq pt-lst (append pt-lst (subseq (send f :vertices) 0 3))))
    (setq 3dp (instance 3dpointcloud :init :points pt-lst))
    (if return-eus3dp
        (list 3dp indices-lst)
      (progn
        (setq ret (make-msg-from-3dpointcloud 3dp))
        (send ret :channels indices-lst)
        ret))))